from numpy import linalg, mat, dot, inf

def cdot(a, b):

    return a.transpose() * M * a

def planefit(points):

    M = mat( [[0, 0, 0, -1.0], [1.0, 0, 0, 0], [0, 1.0, 0, 0], [0, 0, 1.0, 0]] )
    P = sum([p[:-1,] * p[:-1,].transpose() for p in points])

    PM = P * M
    print PM

    [U, S, V] = linalg.svd(PM, 0, 1)
    print V
    V = V.conj().transpose()

    for p in points:
        pp(p)

    print "BEST:"
    pp(V[:, -1], 1)

def c3point(c1, c2, c3):
    return mat( [1, c1, c2, c3, 0.5*(c1 * c1 + c2 * c2 + c3 * c3)] ).transpose()

def pp(p, plane=False):
    if plane:
        print "{0}*e1 + {1}*e2 + {2}*e3 + {3}*ni".format(p[0,0], p[1,0], p[2,0], p[3,0])
    else:
        print "{0}*no + {1}*e1 + {2}*e2 + {3}*e3 + {4}*ni".format(p[0,0], p[1,0], p[2,0], p[3,0], p[4,0])
